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ABSTRACT 

Various  control  algorithms  have  been  developed  for  fleets 
of  autonomous  vehicles.  Many  of  the  successful  control 
algorithms  in  practice  are  behavior-based  control  or  nonlinear 
control  algorithms,  which  makes  analyzing  their  stability 
difficult.  At  the  same  time,  many  system  theoretic  approaches 
for  controlling  a  fleet  of  vehicles  have  also  been  developed. 
These  approaches  usually  use  very  simple  vehicle  models  such 
as  particles  or  point-mass  systems  and  have  only  one  coordinate 
system  which  allows  stability  to  be  proven.  Since  most  of  the 
practical  vehicle  models  are  six-degree-of-freedom  systems 
defined  relative  to  a  body-fixed  coordinate  system,  it  is  difficult 
to  apply  these  algorithms  in  practice. 

In  this  paper,  we  consider  a  formation  regulation  problem 
as  opposed  to  a  formation  control  problem.  In  a  formation 
control  problem,  convergence  of  a  formation  from  random 
positions  and  orientations  is  considered,  and  it  may  need  a 
scheme  to  integrate  multiple  moving  coordinates.  On  the 
contrary,  in  a  formation  regulation  problem,  it  is  not  necessary 
since  small  perturbations  from  the  nominal  condition,  in  which 
the  vehicles  are  in  formation,  are  considered.  A  common  origin 
is  also  not  necessary  if  the  relative  distance  to  neighbors  or  a 
leader  is  used  for  regulation.  Under  these  circumstances,  the 
system  theoretic  control  algorithms  are  applicable  to  a 
formation  regulation  problem  where  the  vehicle  models  have 
six  degrees  of  freedom. 

We  will  use  a  realistic  six-degree-of-freedom  model  and 
investigate  stability  of  a  fleet  using  results  from  decentralized 
control  theory.  We  will  show  that  the  leader- follower  control 
algorithm  does  not  have  any  unstable  fixed  modes  if  the 
followers  are  able  to  measure  distance  to  the  leader.  We  also 
show  that  the  leader-follower  control  algorithm  has  fixed 
modes  at  the  origin,  indicating  that  the  formation  is  marginally 
stable,  when  the  relative  distance  measurements  are  not 
available. 

Multi-vehicle  simulations  are  performed  using  a  hybrid 
leader-follower  control  algorithm  where  each  vehicle  is  given  a 


desired  trajectory  to  follow  and  adjusts  its  velocity  to  maintain 
a  prescribed  distance  to  the  leader.  Each  vehicle  is  modeled  as 
a  three-degree-of- freedom  system  to  investigate  the  vehicle’s 
motion  in  a  horizontal  plane.  The  examples  show  efficacy  of 
the  analysis. 

Keywords:  Formation,  Formation  Control,  Multiple  vehicles, 
Leader-Follower  Algorithm,  Autonomous  Underwater  Vehicles 


I.  INTRODUCTION 

Various  control  algorithms  have  been  developed  for  fleets 
of  autonomous  vehicles.  The  general  idea  is  to  use  relatively 
inexpensive  vehicles  to  cooperatively  solve  a  difficult  problem. 
This  idea  is  best  illustrated  in  a  mine  search  problem.  In  a  mine 
search  problem,  a  large  area  must  be  searched  quickly  and 
carefully  before  humans  can  enter  the  area  safely.  The  cost  of 
each  vehicle  must  be  low  because  there  is  always  some  risk  of 
losing  the  search  vehicles. 

Many  of  the  early  successful  control  algorithms  in  practice 
are  behavior-based  control.  For  example  Balch  and  Arkin 
implemented  a  behavior-based  controller  on  multiple  mobile 
robots  to  move  in  formation  [1].  In  their  work,  several  different 
formation  shapes  and  algorithms  are  evaluated.  Fredslund  and 
Mataric  used  mobile  robots  equipped  with  a  camera  to  move  in 
formation  and  avoid  obstacles  [5]. 

Another  common  approach  is  a  nonlinear  control 
algorithm.  In  [7],  an  artificial  potential  functions  were  used  for 
formation  control.  The  algorithm  was  successfully 
implemented  for  the  control  of  underwater  gliders  [4].  This 
approach  has  an  advantage  over  the  behavior-based  control 
because  stability  can  be  directly  proven  by  Lyapunov’s  method 
[7].  Ihle,  Skjetne  and  Fossen  introduced  and  implemented  a 
nonlinear  control  algorithm  for  a  fleet  of  ships  [6].  In  this 
algorithm,  a  leader  is  absent,  and  formation  flying  is 
accomplished  by  maintaining  desired  distance  to  a  point  called 
the  formation  reference  point  (FRP). 
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At  the  same  time,  many  system  theoretic  approaches  for 
controlling  fleets  of  vehicles  have  also  been  developed.  These 
approaches  usually  use  simple  vehicle  models  such  as  particles 
or  point-mass  systems  and  have  only  one  coordinate  system. 
Stability  of  a  system  can  be  proven  using  the  results  from  graph 
theory  and  decentralized  control  theory  [9].  Although  this 
approach  gives  great  insight  to  a  general  multi-vehicle  problem, 
it  is  difficult  to  apply  these  algorithms  in  practice  since  most  of 
the  practical  vehicle  models  are  nonlinear  six-degree-of- 
freedom  (DOF)  systems  defined  relative  to  a  body-fixed 
coordinate  system. 

In  this  paper,  we  consider  a  formation  regulation  problem 
as  opposed  to  a  formation  control  problem.  In  a  formation 
control  problem,  convergence  of  a  formation  from  random 
positions  and  orientations  is  considered,  and  it  may  need  a 
scheme  to  integrate  multiple  moving  coordinates.  On  the  other 
hand,  in  a  formation  regulation  problem,  small  perturbations 
from  the  nominal  condition,  in  which  the  vehicles  are  in 
formation,  are  considered.  Under  these  circumstances,  the 
system  theoretic  control  approaches  are  applicable  to  a  multiple 
vehicle  problem  where  the  vehicle  models  have  six  degrees  of 
freedom. 

This  paper  is  organized  as  follows.  In  section  II,  a  6DOF 
vehicle  model  and  a  vehicle  coupling  equation  are  developed. 
In  section  III,  we  will  show  that  the  leader-follower  control 
algorithm  does  not  have  any  unstable  fixed  modes  if  the 
followers  are  able  to  measure  a  distance  to  the  leader.  We  also 
show  that  the  leader-follower  control  algorithm  has  fixed 
modes  at  the  origin,  indicating  that  the  formation  is  marginally 
stable,  when  the  relative  distance  measurements  are  not 
available.  In  section  IV,  the  developed  design  and  analysis 
methods  are  applied  to  a  hybrid  leader-follower  control 
algorithm  as  an  example.  Each  vehicle  is  modeled  as  a 
nonlinear  3DOF  system  to  investigate  the  vehicle’s  motion  on  a 
horizontal  plane.  Conclusions  are  given  in  section  V. 


II.  VEHICLE  MODEL  FORMULATION 

We  will  model  our  vehicle  as  a  rigid  body.  Its  dynamics 
are  expressed  by  6DOF  equations  of  motion. 

Consider  a  general  nonlinear  6DOF  model, 

i f=/(o)  (i) 

where  £  is  the  state  vector,  and  //  is  the  control  input  vector. 
Since  it  has  six  degrees  of  freedom,  the  state  vector  is  usually 
given  as  £  =  [u  v  w  p  q  /-]T.  The  right  hand  side  of  Eq.  (1) 
usually  consists  of  damping  terms,  Corioli’s  forces,  control 
forces,  etc. 

It  will  be  convenient  to  define  a  translational  velocity  vector,  xv 
and  a  rotational  velocity  vector,  xr  as 

xv  =  [it  v  w]T  and  xr  =  \_p  q  r\ T.  (2) 

Equation  (1)  is  usually  defined  in  body-fixed  coordinates, 
which  move  with  the  vehicle.  Inertial  position  and  orientation 
of  the  vehicle  are  found  through  coordinate  transformations. 
The  inertial  position  is  found  by  integrating  Eq.  (3), 


where 


Jt  = 


cif/cQ 

si//cO 


-s9 


-  sy/cip  +  c\f/s9s<p  sy/sip  +  cif/ccpsQ 
a//ap  +  sysOsip  -  ci//s<p  +  sipsdcip 
cOsip  cdcip 


where  c(.)  =  cos(.)  and  s(.)  =  sin(.). 

and  the  angles  of  the  vehicle  are  obtained  from 


'll  =  J2Xr  (4) 

where 


~1 

siptQ 

ctptd 

J,  = 

0 

ctp 

-  sip 

0 

s<p/  c6 

cip/cO 

where  t(.)  =  tan{.). 

Stabilization  of  a  fleet  of  vehicles  is  studied  by  Roy,  Saberi, 
and  Herlungston  [9].  They  give  necessary  and  sufficient 
conditions  for  formation  stabilization  of  a  fleet  for  a  given 
observation  graph.  For  stability  of  the  fleet,  existence  of 
unstable  fixed  modes  is  important.  A  fleet  can  be  formation 
stabilized  with  a  decentralized  dynamic  linear  time-invariant 
(LTI)  controller  if  it  does  not  have  any  unstable  fixed  modes, 
and  if  there  exists  a  linear  dynamic  LTI  controller  such  that  all 
eigenvalues  are  placed  in  the  open  left  half  plane  (OLHP). 
Unfortunately,  this  analysis  method  was  developed  using  a 
simple  model  (i.e.,  a  particle)  for  each  vehicle.  Although  it 
provides  a  great  insight  to  the  formation  control  problem,  it 
does  not  mention  some  practical  issues.  For  example,  vehicles 
are  usually  modeled  as  a  six-degree-of-freedom  system  defined 
in  a  body-fixed  coordinate  system,  and  a  position  of  a  vehicle  is 
determined  only  through  coordinate  transformations. 
Moreover,  if  there  are  multiple  vehicles,  there  would  be 
multiple  body-fixed  coordinates  and  inertial  coordinates. 
Difficulties  arise  when  we  have  to  consider  these  coordinates 
together. 

The  main  objective  of  formation  flying  is  that  all  vehicles  keep 
a  prescribed  distance  to  their  neighbors  (or  a  leader)  and  move 
in  a  common  direction.  This  condition  can  be  realized  as  an 
operating  condition  of  the  fleet,  and  this  can  be  viewed  as  a 
regulation  problem.  In  this  problem  setting,  the  system 
theoretic  approaches  for  a  fleet  may  be  applicable.  In  this 
paper,  we  consider  a  leader-follower  system  assuming  that  the 
followers  measure  a  distance  to  the  leader  continuously. 

First  we  derive  linear  equations  for  the  leader.  The  nonlinear  6 
DOF  model  is  linearized  for  a  straight  line  trajectory  to  obtain 
the  following  linear  model 


Co  ^oCo  BoMo 
T„  -  C0C0 


2 


Copyright  ©  2005  by  ASME 


Note  that  the  vehicle  equations  might  have  more  than  six 
equations  to  perform  specific  tasks  (such  as  trajectory 
following). 

A  follower  has  more  states  than  the  leader  because  followers 
regulate  formation  errors  as  well.  The  nonlinear  6  DOF  model 
of  the  ith  follower  is  also  linearized  for  a  straight  line  trajectory 
to  obtain  the  following  linear  model 

l=A^+BlMl  (6) 

Now  define  a  vehicle  separation  vector  as  d0i. 

d0,=d  Q-d,  (7) 

where 

d0  is  a  position  of  the  leader. 
d,  is  a  position  of  the  follower. 

In  the  follower’s  view  point,  the  separation  vector  changes  as 

4  =  R<Xo  ~x„  -xrxdm  (8) 

where 

xvi  and  xvo  are  the  follower  and  the  leader’s 

translational  velocity  in  the  follower’s  body-fixed 
coordinates. 

xv0’  is  the  leader’s  translational  velocity  in  the  leader’s 

body-fixed  coordinates. 

xr  is  the  follower’s  rotational  velocity. 

Rm  is  a  coordinate  transformation  matrix  from  the 
leader  to  the  follower’s  coordinates. 

Note  that  the  transformation  matrix  is  necessary  since  the 
leader’s  velocity  is  defined  with  respect  to  its  body-fixed 
coordinates. 


III.  STABILITY  OF  A  LEADER-FOLLOWER  SYSTEM 

Using  the  models  developed  in  the  previous  section,  we 
investigate  the  stability  of  an  «-vehicle  leader-follower  system. 
The  77-vehicle  leader-follower  system  can  be  written  as 


Z=FZ+GJi 

y  =  m 


(14) 


where  =  [x«T  x/  e<yT  x2  eo?T  . . .  x„T  eo„T]T,  fi  =  \iio  ui T  u2  T 

•  ••  w/]T,  y  =  [>'oTy7Ty/...y„T]Tand 


Now  we  convert  d0i  to  a  deviation  variable  and  define  it  as  a 
formation  error,  ew. 

e0i  =  d0i  -  d,u  (9) 

where  d0j  is  a  desired  nominal  distance  between  the  vehicles. 
Taking  a  time  derivative  and  substituting  it  to  Eq.  (8)  yields 

4  =  R<X«  ~  U,  ~xrx  (eo,  +  d<y )  (10) 

Relationship  between  vehicles  can  be  described  in  a  similar 
way.  However,  if  the  vehicles  move  in  formation,  the  cross 
product  goes  away,  and  the  rotational  matrix  becomes  the 
identity  matrix.  This  is  because  the  local  frames  are  parallel. 
Hence  the  equation  is  simplified  to  the  following  simple  linear 
relationship  of  their  translational  velocities 

4=*v0-*v,-  (11) 


A0  0  0  ■  • •  0  0  0 

0  A,  0  0  0  0  0 

ED,  0  0  0  0  0 

F  =  0  0  0  H2  0  0  !  , 

E  0  0  D2  0  0  0 

:  0  0  0  0  0 

E  0  0  0  Dn  0 

~B0  0  0  •••  0  0  0  " 

0B,  0  0  0  0  0 

o  o  o  o  o  o  : 

G  =  0  0  B2  0  0  0  0  ,  and 

0  0  0  0  0  0  0 

:  0  0  0  0'-.  Bn 

0  0  0  •••  0  0  0 


As  long  as  the  vehicles’  orientation  is  approximately  the  same, 
this  simplification  may  be  valid.  Then  the  state  equation  for  the 
ith  follower  is 
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H  = 


0 

0 

0 

0 


0 


0 

Cn 

0 

0 

0 

0 

0 


0 

0 

~'i: 

o 

o 

0 

0 


0 

0 

■'2 

0 

0 


0  0  0 

0  0  0 

0  0  : 

0  0  0 

\  0  0 

0  c„  0 

0  0  c„2 


This  result  can  also  be  concluded  in  terms  of  fixed  modes  ([9], 
[10]).  The  /7-vehicle  leader-follower  system  has  a  fixed  mode, 
A i,  if  the  following  condition  is  satisfied. 

det{AJ-Fc)=  0  (18) 

The  fleet  can  be  formation  stabilized  if  all  fixed  modes  are  in 
OLHP.  Because  the  closed  loop  system  matrix  is  in  block 
lower  triangular  form,  Eq.  (18)  can  be  written  as 


Notice  that  F  matrix  is  in  lower  block  triangular  form,  and  G 
and  FI  matrices  are  in  block  diagonal  form.  In  other  words,  the 
plant  is  in  input-output  LBT  (lower  block  triangular)  form  [11]. 
For  a  plant  in  this  form,  stability  of  the  overall  system  is 
determined  by  stability  of  subsystems.  Also,  a  decentralized 
controller  may  be  designed  one  by  one  by  neglecting  the  off 
diagonal  elements.  For  instance,  a  decentralized  linear 
quadratic  Gaussian  (LQG)  regulator,  which  is  able  to  tolerate 
nonlinear  coupling  and  other  disturbances,  may  be  designed 
[11].  This  property  makes  the  leader- follower  system  a 
preferred  system  in  practice  especially  if  an  advanced  controller 
is  required  for  controlling  each  vehicle. 

We  assume  that  the  leader  measures  all  of  its  states  (i.e.,  C0I  = 
/),  and  the  followers  measure  all  of  its  states  and  distance  to  the 
leader  (i.e.,  C,7  =  Ci2  =  /).  The  following  controller  is  used  for 
the  leader 


det(XJ  -  4,  -  B„KmCm )  •  det 


II- 


D, 


f 

[A  +BK.C.  5.S:,C,T| 

det\  aJ  - 

n  n  n\  nl  it  n  2  n  2 

.  A,  o  JJ 

0  JJ- 


which  indicates  that  fixed  modes  of  the  system  are  a  union  of 
uncontrollable  and  unobservable  modes  of  each  vehicle. 
Therefore,  this  system  obviously  has  no  fixed  modes,  and  the 
fleet  can  be  formation  stabilized.  Another  observation  is  that 
when  the  followers  become  incapable  of  measuring  the  distance 
to  the  leader,  the  fleet  would  have  multiple  eigenvalues  at  the 
origin.  In  other  words,  the  formation  becomes  marginally 
stable.  This  can  easily  be  seen  by  substituting  Ci2  matrix  with 
the  zero  matrices. 


Mo  =  K0y0 


(15) 


and  the  following  controller  is  used  for  the  ith  follower. 


Mi  =  Kiyi  (16) 

where 

K,=[Kn  Kl2] 


Closed-loop  equation  for  this  system  is 


?  =  A? 

where 


(17) 


A0  +  B0KOICOI 
0 
E 
0 
E 

E 


0 

At+BtKnCu 

Di 

0 

0 

0 

0 


0  •••  0  0  0 

B,KI2C,2  0  0  0  0 

0  0  0  0  0 

0  A2  +  B2K2tC2,  B .  K.,X',2  0  : 

0  D2  0  0  0 

0  0  o  B„K„2c.2 

0  •••  0  D„  0 


Because  Fc  matrix  of  the  closed  loop  equation  is  in  block  lower 
triangular  form,  eigenvalues  of  overall  system  are  a  union  of 
eigenvalues  of  subsystems.  Flence  stability  of  the  fleet  can  be 
established  if  each  vehicle  can  be  stabilized.  Moreover,  since 
we  assumed  that  each  vehicle  was  controllable  and  observable, 
the  fleet  can  be  stabilized  with  proper  feedback  controllers. 
This  result  is  not  surprising  because  some  leader-follower 
controllers  have  been  designed  successfully  without  formal 
analysis  (for  example  [3,  8]). 


IV.  EXAMPLE:  A  HYBRID  LEADER-FOLLOWER 
ALGORITHM 

We  consider  a  hybrid  leader-follower  algorithm  introduced 
in  [3].  This  algorithm  is  developed  for  a  fleet  of  autonomous 
underwater  vehicles  (AUVs)  searching  for  underwater  mines. 
In  this  algorithm  it  is  assumed  that  all  vehicles  know  their 
inertial  position,  and  the  leader  broadcasts  its  inertial  position 
to  the  followers.  Only  communication  required  for  formation 
flying  is  the  position  of  the  leader.  Each  vehicle  follows  a 
given  search  trajectory  (or  path),  and  the  followers  adjust  their 
velocity  to  maintain  a  prescribed  distance  to  the  leader. 

In  this  example,  a  3  DOF  model  of  a  REMUS  given  in  [8]  is 
adapted.  A  linear  state  space  model  of  the  vehicle  is  given  in 
Eq.  (19). 


U 

0.37 

0 

0 

0~ 

U 

0.03 

o  n 

V 

0 

-2.78 

-0.63 

0 

V 

+ 

0 

0.28 

r 

0 

-5.00 

-1.97 

0 

r 

0 

-1.60 

>_ 

0 

0 

1 

0 

y_ 

0 

0 

(19) 


where 

u  and  v  are  forward  and  lateral  velocities  of  the  vehicle 
in  m/sec  defined  in  body  fixed  coordinates. 
r  is  yaw  rate  in  rad/sec. 
i//  is  yaw  angle  in  rad. 
uprop  is  a  propeller  thrust  force  in  N 
3r  is  a  rudder  angle  in  rad 
The  operating  condition,  xa,  is  xQ  =  [1.5  0  0  0]. 

Now  we  define  a  deviation  of  heading  from  a  search  path,  6 , 
as 


6  =  ys-p 


(20) 
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Figure  1:  A  Hybrid  Leader-Follower  Algorithm 

where  P  is  an  angle  of  the  path  with  respect  to  the  inertial 
coordinates  (see  Fig.  1). 

We  take  a  time  derivative  of  Eq.  (20)  and  obtain 


9  -  r 


(21) 


where  P  was  assumed  to  be  constant. 

A  kinematic  equation  for  the  perpendicular  distance  from  the 
path,  dx,  is  defined  as 

dL  =u  sin  9  +  v  cos  9  (22) 

Linearizing  Eq.  (22)  at  the  previously  defined  operating  point 
yields 


Note  that  only  the  x-componcnt  of  the  formation  error  was 
augmented. 

Equation  (26)  is  written  compactly  as 


l  =  +  Bp , 

=  Df,  +  £c„ 


(27) 


where  $  =  [u,  v,  r,  9 ,  dM]T ,  ju,  =  [ upropJ  d,.,]  '  ,  D  =  [-1  0  0  0  0],  and 

£=[1  0  0  0  0], 

Decentralized  LQR  Design 

Assuming  that  each  vehicle  is  able  to  measure  all  local  states,  a 
linear  quadratic  regulator  (LQR)  is  designed  for  each  vehicle 
separately. 

The  LQ  control  law  for  the  leader  is 

Mo  =  (28) 


d ,  =  v  + 1.5 9 


(23) 


Augmenting  Eq.  (21)  and  Eq.  (23)  with  Eq.  (19)  yields  a  state 
space  model  for  the  leader. 
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We  denote  Eq.  (24)  compactly  as 

£0=A{0+Bp0 


(24) 


(25) 


where 

K0  =-R0'BTP0 

where  Po  is  a  positive  definite  solution  of  the  algebraic 
control  Riccati  equation 

0  =  P0A  +  ATP0-P0BR0'BTP0+Q0 

The  control  law  given  in  Eq.  (28)  minimizes  a  quadratic  cost 
function 


J0=[  (<for0o<?o 

For  the  follower,  we  will  ignore  the  coupling  term,  E^0,  and 
design  an  LQR  for  Eq.  (27)  to  minimize  a  cost  function. 


J 


Q, 


+  [ij  Rijui 


dt 


Equation  for  the  formation  error  is  added  to  Eq.  (24)  to  obtain  a 
follower  model  given  in  Eq.  (26). 


The  solution  to  this  LQR  problem  is 
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where 


20 


[Kn  Ka]  =  -Ri  '[bt  o]p 


where  P,  is  a  positive  definite  solution  of  the  algebraic 
control  Riccati  equation 
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Simulation  Result 

In  this  section,  we  will  perform  a  simulation  with  three 
vehicles:  a  leader  and  two  followers.  In  order  to  see  usefulness 
and  effectiveness  of  the  design  methodology,  a  nonlinear  3 
DOF  model  was  used  for  each  vehicle.  Because  each  vehicle 
has  a  trajectory  to  follow,  formation  flying  is  accomplished  if 
the  followers  maintain  a  prescribed  forward  distance  to  the 
leader.  The  desired  distance  was  set  to  10m  in  x,- -direction  for 
this  example  while  the  trajectories  were  separated  by  40m. 
Figure  2  shows  the  vehicle  trajectories  (Triangles)  and  search 
paths  (Dashed  lines).  Although  the  vehicles  were  initially 
placed  at  random  position  and  orientation,  they  converged  to 
their  assigned  search  trajectory  and  maintained  prescribed 
distance  to  the  leader.  Figure  3  shows  formation  errors. 
(Positive  error  means  a  follower  is  behind  the  desired  position 
and  vise  versa.)  The  simulation  result  indicates  that  the 
assumptions  are  valid,  and  the  controller  design  methodology  is 
simple,  robust,  and  efficient.  Even  if  the  followers  lose 
communication  with  the  leader,  the  followers  would  still  follow 
the  trajectory  since  the  coupling  only  causes  their  velocity  to 
change. 


Time  (sec) 

Figure  3:  Formation  Errors 


V.  CONCLUSION 

In  this  paper,  a  formation  control  problem  was  formulated 
into  a  formation  regulation  problem,  and  a  design  methodology 
of  a  simple  and  robust  decentralized  control  for  a  fleet  of 
vehicles  was  introduced.  Stability  of  the  system  was  shown  in 
terms  of  existence  of  unstable  fixed  modes.  It  was  shown  that 
the  stability  of  the  ^-vehicle  leader-follower  system  was 
achieved  by  stabilizing  each  vehicle  separately.  This  property 
would  allow  control  engineers  to  design  an  advanced  controller 
for  higher  performances.  As  an  example,  a  hybrid  leader- 
follower  system  was  designed.  Simulation  results  showed  that 
the  design  and  analysis  methods  are  valid  and  useful. 
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